/* 简介：cothread 是一个轻量级协程调度器，由纯C语言实现，易于移植到各种单片机。
 * 同时，由于该调度器仅仅运行在一个实际线程中，所以它也适用于服务器高并发场景。
 *
 * 版本: 1.0.0   2019/02/25
 *
 * 作者: 覃攀 <qinpan1003@qq.com>
 *
 */

#include "rtos.h"

static ccb_t ccb_table[COTHREAD_NR];

static ccb_t *free_ccb_list = NULL;
static ccb_t *ready_ccb_list[THREAD_PRIO_NR];
static ccb_t *delay_ccb_list = NULL;
static ccb_t *wait_ccb_list = NULL;

ccb_t *current_thread = NULL;

static int cothread_inited = 0;
static unsigned int current_tick = 0;
static unsigned int sched_tick = 0;
static unsigned long sched_nr = 0;

struct cothread_stat {
    int free_nr;
    int ready_nr[THREAD_PRIO_NR];
    int delay_nr;
    int wait_nr;

    unsigned int sched_tick_max;
    void *sched_tick_max_fun;

    unsigned long sched_nr;

    /* CPU 使用率 = idle 的当前计数值 / 最大计数值 */
    unsigned long idle_counter_current;
    unsigned long idle_counter_max;
};

static struct cothread_stat cothread_runtime_state;

static unsigned long idle_counter = 0;
static unsigned int idle_tick_timeout = 0;
static unsigned int idle_counter_last = 0;

static void (*idle_hook_table[OS_IDLE_HOOK_NR])(void);
static int idle_hook_count = 0;

/* 注册 idle 任务钩子函数，用于对优先级无要求的场景 */
int register_idle_hook(void (*hook)(void))
{
    if (hook == NULL)
    {
        LOG_ERR("idle hook null %d, %d\n", idle_hook_count, OS_IDLE_HOOK_NR);
        return -1;
    }

    if (idle_hook_count >= OS_IDLE_HOOK_NR)
    {
        LOG_WARN("idle hook full %d, %d, %p\n", 
            idle_hook_count, OS_IDLE_HOOK_NR, hook);
        return -1;
    }

    idle_hook_table[idle_hook_count++] = hook;

    return 0;
}

static void do_idle_hook(void)
{
    int i;
    
    for (i = 0; i < idle_hook_count; i++)
        idle_hook_table[i]();
}

ccb_t *ccb_idle = NULL;

static coresult_t idle_thread(ccb_t *ccb)
{
    thread_start();

    idle_tick_timeout = current_tick + 1000;
    idle_counter_last = idle_counter;

    while (1)
    {
        idle_counter++;

        if (current_tick - idle_tick_timeout < (1u << 31))
        {
            cothread_runtime_state.idle_counter_current 
                    = idle_counter - idle_counter_last;

            if (cothread_runtime_state.idle_counter_current > cothread_runtime_state.idle_counter_max)
                cothread_runtime_state.idle_counter_max = cothread_runtime_state.idle_counter_current;
            
            idle_tick_timeout = current_tick + 1000;
            idle_counter_last = idle_counter;
        }

        do_idle_hook();
        thread_yeild();
    }

    thread_end();
}

/* 记录空闲时候 idle 计数 */
static void idle_max_counter(void)
{
    if (ccb_idle == NULL)
    {
        LOG_ERR("ccb_idle NULL.\n");
        return;
    }

    idle_counter = 0;
    current_tick = 0;
    while (current_tick < 1000)
    {
        idle_thread(ccb_idle);
        cothread_runtime_state.sched_nr++;
    }
    cothread_runtime_state.idle_counter_max = idle_counter;
    cothread_runtime_state.idle_counter_current = 0;
    idle_counter = 0;
    idle_counter_last = 0;
    cothread_runtime_state.sched_nr = 0;
}

static void (*sched_hook_table[OS_SCHED_HOOK_NR])(void);
static int sched_hook_count = 0;

/* 注册任务调度钩子函数，用于对优先级要求最高，能及时响应中断的场景 */
int register_sched_hook(void (*hook)(void))
{
    if (hook == NULL)
    {
        LOG_ERR("sched hook null %d, %d\n", sched_hook_count, OS_SCHED_HOOK_NR);
        return -1;
    }

    if (sched_hook_count >= OS_SCHED_HOOK_NR)
    {
        LOG_WARN("sched hook full %d, %d, %p\n", 
                    sched_hook_count, OS_SCHED_HOOK_NR, hook);
        return -1;
    }    

    sched_hook_table[sched_hook_count++] = hook;

    return 0;
}

static void do_sched_hook(void)
{
    int i;
    
    for (i = 0; i < sched_hook_count; i++)
        sched_hook_table[i]();
}

int cothread_init(void)
{
    int i = 0;
    ccb_t *ccb = NULL;

    if (cothread_inited)
    {
        LOG_ERR("cothread_init:reinit\n");
        return -1;
    }
    
    free_ccb_list = NULL;
    delay_ccb_list = NULL;
    wait_ccb_list = NULL;

    memset(ready_ccb_list, 0, sizeof(ready_ccb_list));
    memset(ccb_table, 0, sizeof(ccb_table));
    memset(&cothread_runtime_state, 0, sizeof(cothread_runtime_state));
    
    for (i = 0; i < COTHREAD_NR; i++)
    {
        ccb = ccb_table + i;

        if (i != (COTHREAD_NR - 1))
            ccb->next = ccb_table + i + 1;
        
        if (i == 0)
            free_ccb_list = ccb;
        else
            ccb->prev = ccb_table + i - 1;

        cothread_runtime_state.free_nr++;
    }
    
    sched_hook_count = 0;
    idle_hook_count = 0;
    
    ccb_idle = cothread_create(idle_thread, NULL, THREAD_PRIO_LOW);

    cothread_inited = 1;
    return 0;
}

ccb_t *cothread_create(coresult_t (*fun)(ccb_t *ccb), void *arg, cothread_prio_t prio)
{
    irq_state_t irq_stat;
    ccb_t *ccb = NULL;

    /* 从 free 队列头取一个节点 */
    if (free_ccb_list == NULL)
    {
        LOG("[war] ccb lack %p, %d.\n", fun, prio);
        return NULL;
    }

    cothread_enter_critical();

    ccb = free_ccb_list;
    if (ccb->status != COTHREAD_STATUS_NONE)
    {
        LOG("[bug] ccb status invalid %p, %d.\n", fun, prio);
        cothread_exit_critical();
        return NULL;
    }

    if (fun == NULL)
    {
        LOG("[bug] new fun is NULL.\n");
        cothread_exit_critical();
        return NULL;
    }

    free_ccb_list = ccb->next;
    cothread_runtime_state.free_nr--;

    ccb->prio = prio;
    ccb->fun = fun;
    ccb->arg = arg;
    ccb->status = COTHREAD_STATUS_READY;

    /* 节点插入    ready 队列 */
    ccb->prev = NULL;
    ccb->next = ready_ccb_list[ccb->prio];

    if (ready_ccb_list[ccb->prio] != NULL)
        ready_ccb_list[ccb->prio]->prev = ccb;
    ready_ccb_list[ccb->prio] = ccb;

    cothread_runtime_state.ready_nr[ccb->prio]++;

    cothread_exit_critical();
    return ccb;
}

/* 注意：线程只能删除自己，不能删除其他线程，否则调度器遍历会出错 */
int cothread_delete(ccb_t *ccb)
{
    irq_state_t irq_stat;

    if (ccb->status != COTHREAD_STATUS_READY)
    {
        LOG("[bug] delete thread status invalid %p, %d.\n", ccb->fun, ccb->status);
        return -1;
    }

    cothread_enter_critical();

    /* 从队列中移除 */
    if (ccb == ready_ccb_list[ccb->prio])
        ready_ccb_list[ccb->prio] = ccb->next;
    else
        ccb->prev->next = ccb->next;

    if (ccb->next != NULL)
        ccb->next->prev = ccb->prev;

    cothread_runtime_state.ready_nr[ccb->prio]--;

    memset(ccb, 0, sizeof(*ccb));

    /* 放入   free 队列 */
    ccb->prev = NULL;
    ccb->next = free_ccb_list;

    if (free_ccb_list != NULL)
        free_ccb_list->prev = ccb;
    free_ccb_list = ccb;

    cothread_runtime_state.free_nr++;

    cothread_exit_critical();

    return 0;
}

void cothread_yeild(ccb_t *ccb)
{
    if (ccb->status != COTHREAD_STATUS_READY)
        LOG("[bug] yeild thread status invalid %p, %d.\n", ccb->fun, ccb->status);
}

int cothread_sleep(ccb_t *ccb, unsigned int tick)
{
    irq_state_t irq_stat;
    ccb_t *ccb_tmp;

    if (ccb->status != COTHREAD_STATUS_READY)
    {
        LOG("[bug] sleep thread status invalid %p, %d.\n", ccb->fun, ccb->status);
        return -1;
    }

    cothread_enter_critical();

    /* 从 ready 队列中移除  */
    if (ccb == ready_ccb_list[ccb->prio])
        ready_ccb_list[ccb->prio] = ccb->next;
    else
        ccb->prev->next = ccb->next;
    
    if (ccb->next != NULL)
        ccb->next->prev = ccb->prev;
    
    ccb->status = COTHREAD_STATUS_DELAY;
    ccb->timeout_tick = current_tick + tick;
    cothread_runtime_state.ready_nr[ccb->prio]--;

    /* 放入 delay   队列， delay 队列在放入的时候进行排序 */
    cothread_runtime_state.delay_nr++;
    ccb->prev = NULL;
    ccb->next = NULL;

    /* delay 队列为空，直接放入链表头 */
    if (delay_ccb_list == NULL)
    {
        delay_ccb_list = ccb;
        cothread_exit_critical();
        return 0;
    }

    /* delay 队列非空，升序插入链表 */
    ccb_tmp = delay_ccb_list;
    while (ccb_tmp->next != NULL && ccb_tmp->timeout_tick < ccb->timeout_tick)
        ccb_tmp = ccb_tmp->next;

    if (ccb_tmp->timeout_tick < ccb->timeout_tick)
    {
        ccb_tmp->next = ccb;
        ccb->prev = ccb_tmp;
    }
    else
    {
        ccb->next = ccb_tmp;
        ccb->prev = ccb_tmp->prev;
        
        ccb_tmp->prev = ccb;
        if (ccb_tmp == delay_ccb_list)
            delay_ccb_list = ccb;
        else
            ccb->prev->next = ccb;
    }

    cothread_exit_critical();
    return 0;
}

/* 线程唤醒后自己通过 ccb->event_wakeup 判断是否超时 */
int cothread_wait(ccb_t *ccb, unsigned int event_mask, unsigned int tick)
{
    int ret;
    irq_state_t irq_stat;
    
    if (ccb->status != COTHREAD_STATUS_READY)
    {
        LOG("[bug] wait thread status invalid %p, %d.\n", ccb->fun, ccb->status);
        return -1;
    }

    ccb->event_mask = event_mask;

    cothread_enter_critical();

    /* 信号已到达，直接返回 */
    if (ccb->event_mask & ccb->event_signaled)
    {
        ccb->event_wakeup = 1;
        cothread_exit_critical();
        return 0;
    }

    if (tick != 0)
    {
        /* 定时等待，放入 delay 队列 */
        ret = cothread_sleep(ccb, tick);
        cothread_exit_critical();
        return ret;
    }
    else
    {
        /* 永久等待，放入   wait 队列，便于统计 */
        /* 从 ready 队列中移除 */
        if (ccb == ready_ccb_list[ccb->prio])
            ready_ccb_list[ccb->prio] = ccb->next;
        else
            ccb->prev->next = ccb->next;
        
        if (ccb->next != NULL)
            ccb->next->prev = ccb->prev;

        cothread_runtime_state.ready_nr[ccb->prio]--;

        ccb->status = COTHREAD_STATUS_WAIT;

        /* 放入   wait 队列 */
        ccb->prev = NULL;
        ccb->next = wait_ccb_list;

        if (wait_ccb_list != NULL)
            wait_ccb_list->prev = ccb;
        wait_ccb_list = ccb;

        cothread_runtime_state.wait_nr++;

        cothread_exit_critical();
        return 0;
    }
}

/* 注意：只能在线程间发送信号，不能从中断往线程发送信号，因为没有互斥机制 */
/* 中断可以通过软中断机制向线程传递信息，软中断运行在调度器中，不需要和线程互斥 */
int cothread_signal(ccb_t *ccb, unsigned int event_mask)
{
    irq_state_t irq_stat;

    cothread_enter_critical();
    ccb->event_signaled |= event_mask;

    if ((ccb->event_mask & ccb->event_signaled) == 0
        || (ccb->status != COTHREAD_STATUS_DELAY && ccb->status != COTHREAD_STATUS_WAIT))
    {
        cothread_exit_critical();
        return 0;
    }

    /* 从 delay 或者 wait 队列中移除 */
    if (ccb == delay_ccb_list)
        delay_ccb_list = ccb->next;
    else if (ccb == wait_ccb_list)
        wait_ccb_list = ccb->next;
    else
        ccb->prev->next = ccb->next;
    
    if (ccb->next != NULL)
        ccb->next->prev = ccb->prev;

    if (ccb->status == COTHREAD_STATUS_DELAY)
        cothread_runtime_state.delay_nr--;
    else
        cothread_runtime_state.wait_nr--;

    ccb->event_wakeup = 1;
    ccb->status = COTHREAD_STATUS_READY;
    
    /* 放入 ready 队列 */
    ccb->prev = NULL;
    ccb->next = ready_ccb_list[ccb->prio];

    if (ready_ccb_list[ccb->prio] != NULL)
        ready_ccb_list[ccb->prio]->prev = ccb;
    ready_ccb_list[ccb->prio] = ccb;

    cothread_runtime_state.ready_nr[ccb->prio]++;

    cothread_exit_critical();
    return 0;
}

static void cothread_handle_ready_quene_prio(ccb_t *ccb)
{
    coresult_t ret;
    ccb_t *ccb_tmp = NULL;
    unsigned int tick_tmp;

    while (ccb)
    {
        if (ccb->status != COTHREAD_STATUS_READY)
        {
            LOG("[bug] ready thread status invalid %p, %d\n", ccb->fun, ccb->status);
            return;
        }

        if (ccb->fun == NULL)
        {
            LOG("[bug] ready thread fun is NULL\n");
            return;
        }

        ccb_tmp = ccb;
        ccb = ccb->next;

        current_thread = ccb_tmp;
        tick_tmp = current_tick;
        ret = ccb_tmp->fun(ccb_tmp);
        tick_tmp = current_tick - tick_tmp;
        current_thread = NULL;

        if (tick_tmp > (OS_HZ / 10))
            LOG("[warn][%p][%u] sched too slow.\n", ccb_tmp->fun, tick_tmp);

        if (tick_tmp > cothread_runtime_state.sched_tick_max)
        {
            cothread_runtime_state.sched_tick_max = tick_tmp;
            cothread_runtime_state.sched_tick_max_fun = ccb_tmp->fun;
        }

        if (ret == STATUS_DONE)
            cothread_delete(ccb_tmp);

        cothread_runtime_state.sched_nr++;
    }
}

static void cothread_handle_ready_quene(void)
{
    int i;
    
    for (i = 0; i < THREAD_PRIO_NR; i++)
    {
        if (ready_ccb_list[i] != NULL)
        {
            cothread_handle_ready_quene_prio(ready_ccb_list[i]);
            return;
        }
    }
}

static void cothread_handle_delay_quene(void)
{
    ccb_t *ccb;
    irq_state_t irq_stat;
    
    cothread_enter_critical();
    ccb = delay_ccb_list;

    while (ccb)
    {
        /* 类似 time_before */
        if (time_before(current_tick, ccb->timeout_tick))
            break;
        
        /* 从 delay 队列中移除 */
        delay_ccb_list = ccb->next;
        
        if (ccb->next != NULL)
            ccb->next->prev = ccb->prev;
        cothread_runtime_state.delay_nr--;

        ccb->event_wakeup = 0;
        ccb->status = COTHREAD_STATUS_READY;
        
        /* 放入 ready 队列 */
        ccb->prev = NULL;
        ccb->next = ready_ccb_list[ccb->prio];

        if (ready_ccb_list[ccb->prio] != NULL)
            ready_ccb_list[ccb->prio]->prev = ccb;
        ready_ccb_list[ccb->prio] = ccb;
        cothread_runtime_state.ready_nr[ccb->prio]++;

        ccb = delay_ccb_list;
    }

    cothread_exit_critical();
}

void cothread_loop_once(void)
{
    static unsigned int last_loop_tick = 0;

    do_sched_hook();
    
    cothread_handle_ready_quene();
    
    if (last_loop_tick == current_tick)
        return;
    
    cothread_handle_delay_quene();
    last_loop_tick = current_tick;
}

void cothread_start(void)
{
    system_timer_start();
    idle_max_counter();

    while (1)
    {
        cothread_loop_once();
    }
}

void system_tick(void)
{
    current_tick++;

    /* 检测调度器是否卡住 */
    if (sched_nr != cothread_runtime_state.sched_nr)
    {
        sched_nr = cothread_runtime_state.sched_nr;
        sched_tick = current_tick;
    }
    /* 0.1 秒警告，日志放入log线程的输出buff，连续输出 10次 */
    else if (time_before(sched_tick + OS_HZ / 10, current_tick)
        && time_before(current_tick, sched_tick + OS_HZ / 10 + 10))
    {
        LOG("[warn][%p][%u] long time not sched.\n", 
            current_thread->fun, current_tick - sched_tick);
    }
    /* 1 秒警告，调度器卡死，log线程也得不到调度，所以要直接操作硬件进行日志输出 */
    else if (time_before(sched_tick + OS_HZ, current_tick)
        && time_before(current_tick, sched_tick + OS_HZ + 10))
    {
        LOG_POLL("[bug][%p][%u] too long time not sched.\n", 
            current_thread->fun, current_tick - sched_tick);
    }
}

unsigned int get_system_tick(void)
{
    return current_tick;
}

void cothread_show_stat(void)
{
    int i;
    struct cothread_stat stat = cothread_runtime_state;
    
    LOG("os show:\n\n"
                "  tick:%u\n"
                "  cpu:%lu%%\n"
                "  sched:%lu\n"
                "  slow:%p,%u\n"
                "\n"
                "  free_quene:%d/%d\n"
                "  delay_quene:%d\n"
                "  wait_quene:%d\n",
                current_tick, 
                100 - (100 * stat.idle_counter_current / stat.idle_counter_max),
                stat.sched_nr,
                stat.sched_tick_max_fun, stat.sched_tick_max,
                stat.free_nr, COTHREAD_NR,
                stat.delay_nr,
                stat.wait_nr);

    for (i = 0; i < THREAD_PRIO_NR; i++)
        LOG("  ready_quene[%d]:%d\n", i, stat.ready_nr[i]);

    LOG("\n");
}

